
/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : sensor_sync.h
 *@brief   : sensor_sync
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/10/07
 *
 */

#ifndef SENSOR_SYNC_H
#define SENSOR_SYNC_H
#include <iostream>
#include <vector>

#include "common/state.h"
#include "common/se2.h"
#include "common/map_builder.h"
#include "feature/feature.h"

namespace sensor {

using namespace feature;

struct Measurement {
    Measurement() {};

    double timestamp;
    std::vector<FeaturePoint> feature_data;
    std::deque<ImuData> imu_data;
    std::deque<WheelData> wheel_data;       
};

class SensorSync {
public:
   SensorSync();
   void AddImuData(const ImuData& imu_data);
   void AddWheelOdometryData(const WheelData& wheel_data);
   void AddFeatureData(const double& timestamp, const std::vector<FeaturePoint>& feature_data);
   bool Sync();

   std::vector<FeaturePoint> GetFeatureData() {
      return measures_.feature_data;
   }

   std::deque<ImuData> GetImuData() {
      return measures_.imu_data;
   }
   
   std::deque<WheelData> GetWheelData() {
      return measures_.wheel_data;
   }

private:
   std::deque<ImuData> imu_queue_;
   std::deque<WheelData> wheel_queue_;

   std::deque<double> time_queue_;
   std::deque<std::vector<FeaturePoint>> feature_queue_;

   double last_timestamp_odom_ = 0;
   double last_timestamp_imu_ = -1.0;
   double last_timestamp_feature_ = 0;

   bool feature_pushed_ = false;

   Measurement measures_;
};

}
#endif 
